{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "using RigidBodyDynamics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Model definition"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We'll just use the double pendulum model, loaded from a URDF:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Spanning tree:\n",
       "Vertex: world (root)\n",
       "  Vertex: upper_link, Edge: shoulder\n",
       "    Vertex: lower_link, Edge: elbow\n",
       "No non-tree joints."
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "urdf = Pkg.dir(\"RigidBodyDynamics\", \"test\", \"urdf\", \"Acrobot.urdf\")\n",
    "mechanism = parse_urdf(Float64, urdf)\n",
    "remove_fixed_tree_joints!(mechanism)\n",
    "mechanism"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Controller"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's write a simple controller that just applies $10 \\sin(t)$ at the elbow joint and adds some damping at the shoulder joint:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "simple_control! (generic function with 1 method)"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "shoulder, elbow = joints(mechanism)\n",
    "function simple_control!(torques::AbstractVector, t, state::MechanismState)\n",
    "    torques[velocity_range(state, shoulder)] .= -1 .* velocity(state, shoulder)\n",
    "    torques[velocity_range(state, elbow)] .= 10 * sin(t)\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basic simulation can be done using the `simulate` function. We'll first create a `MechanismState` object, and set the initial joint configurations and velocities:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "state = MechanismState(mechanism)\n",
    "zero_velocity!(state)\n",
    "configuration(state, shoulder)[:] = 0.7\n",
    "configuration(state, elbow)[:] = -0.8;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can simply call `simulate`, which will return a tuple consisting of:\n",
    "* simulation times (a `Vector` of numbers)\n",
    "* joint configuration vectors (a `Vector` of `Vector`s)\n",
    "* joint velocity vectors (a `Vector` of `Vector`s)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "final_time = 10.\n",
    "ts, qs, vs = simulate(state, final_time, simple_control!; Δt = 1e-3);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For access to lower-level functionality, such as different ways of storing or visualizing the data generated during the simulation, it is advised to simply pattern match the basic `simulate` function."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For visualization, we'll use [`RigidBodyTreeInspector`](https://github.com/rdeits/RigidBodyTreeInspector.jl).\n",
    "\n",
    "(Note: the `#NBSKIP` comments are to skip these cells during `Pkg.test(\"RigidBodyDynamics\")`)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\u001b[1m\u001b[36mINFO: \u001b[39m\u001b[22m\u001b[36mPrecompiling module RigidBodyTreeInspector.\n",
      "\u001b[39m"
     ]
    }
   ],
   "source": [
    "#NBSKIP\n",
    "using RigidBodyTreeInspector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Open the viewer application if it isn't open already:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "DrakeVisualizer.any_open_windows() || (DrakeVisualizer.new_window(); sleep(1));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Load the mechanism's geometry into the visualizer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "geometries = parse_urdf(urdf, mechanism)\n",
    "vis = Visualizer(mechanism, geometries);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And animate:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "RigidBodyTreeInspector.animate(vis, mechanism, ts, qs; realtimerate = 1.);"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.2",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
